clc;clear;close all;

% 手
% alpha = [pi/2, -pi/2, pi/2, pi/2, pi/2]';
% q = [2.6, -4, 2.8, -3.17, 1.57]';
% u51_arm = J_la_T05_q1(q, alpha);

% 右腿
%{
alpha = [pi/2, pi/2, -pi/2, -1.4835]';
q = [pi/2, pi/2, pi/2, 0]';
u11_rl = J_rl_T01_q1(q, alpha);
% [-1,-6.12323399573677e-17,0,0;3.74939945665464e-33,-6.12323399573677e-17,0,0;6.12323399573677e-17,-1,0,0;0,0,0,0]
u21_rl = J_rl_T02_q1(q, alpha);
% [-6.12323399573677e-17,1,6.12323399573677e-17,0;-3.74939945665464e-33,-3.74939945665464e-33,6.12323399573677e-17,0;-6.12323399573677e-17,-6.12323399573677e-17,1,0;0,0,0,0]
u31_rl = J_rl_T03_q1(q, alpha);
% [0,6.12323399573677e-17,1,0.397200000000000;-6.12323399573677e-17,0,0,0;-1,0,0,0;0,0,0,0]
u41_rl = J_rl_T04_q1(q, alpha);
% [0,-0.996192094811395,0.0871854932616892,0.397200000000000;-6.12323399573677e-17,-1.22565334042773e-66,-1.40044647685650e-65,0;-1,-2.00164380665687e-50,-2.28710266148826e-49,0;0,0,0,0]
u22_rl = J_rl_T02_q2(q, alpha);
% [-6.12323399573677e-17,6.12323399573677e-17,0,0;-1.22464679914735e-16,1,0,0;-1,-1.22464679914735e-16,0,0;0,0,0,0]
u32_rl = J_rl_T03_q2(q, alpha);
% [0,6.12323399573677e-17,6.12323399573677e-17,2.43214854310664e-17;6.12323399573677e-17,1.22464679914735e-16,1,0.397200000000000;-6.12323399573677e-17,1,-1.22464679914735e-16,-4.86429708621329e-17;0,0,0,0]
u42_rl = J_rl_T04_q2(q, alpha);
% [0,-5.56606012495831e-17,6.63377447750841e-17,2.43214854310664e-17;6.12323399573677e-17,-0.996192094811395,0.0871854932616893,0.397200000000000;-6.12323399573677e-17,0.0871854932616893,0.996192094811395,-4.86429708621329e-17;0,0,0,0]
u33_rl = J_rl_T03_q3(q, alpha);
% [0,1,0,0;1,1.23259516440783e-32,0,0;-1.22464679914735e-16,-1.23259516440783e-32,0,0;0,0,0,0]
u43_rl = J_rl_T04_q3(q, alpha);
% [0,0.0871854932616892,0.996192094811395,0;1,6.53786482127089e-34,7.47024419801899e-33,0;-1.22464679914735e-16,-1.07464417400870e-33,-1.22790155888583e-32,0;0,0,0,0]
u44_rl = J_rl_T04_q4(q, alpha);
% [0,1,0,0;0.0871854932616893,7.49879891330929e-33,0,0;0.996192094811395,-1.23259516440783e-32,0,0;0,0,0,0]
%}

% 左手
alpha = [pi/2, -pi/2, pi/2, pi/2, pi/2]';
q = [pi/2, -1.5359, 0, pi, pi/2]';

% u11_la = J_la_T01_q1(q, alpha);
% u21_la = J_la_T02_q1(q, alpha);
% u22_la = J_la_T02_q2(q, alpha);
% u31_la = J_la_T03_q1(q, alpha);
% u32_la = J_la_T03_q2(q, alpha);
% u33_la = J_la_T03_q3(q, alpha);
% u41_la = J_la_T04_q1(q, alpha);
% u42_la = J_la_T04_q2(q, alpha);
% u43_la = J_la_T04_q3(q, alpha);
% u44_la = J_la_T04_q4(q, alpha);
% u51_la = J_la_T05_q1(q, alpha);
% u52_la = J_la_T05_q2(q, alpha);
% u53_la = J_la_T05_q3(q, alpha);
% u54_la = J_la_T05_q4(q, alpha);
u55_la = J_la_T05_q5(q, alpha);